#include "quyufugai/quyufugai.hpp"

quyufugai::quyufugai():Node("quyufugai") {
    initialize_parameters();

    errs = 0.5;
    speed = 4.0;
  
    service_status = true;


    if (width < 0 || height < 0 || spacing < 0 || gap < 0|| vehicle_num < 0)
    {
        RCLCPP_ERROR(this->get_logger(), "Invalid parameters fialed， please check the parameters");
    }
    service_status_sub_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(
            namespaces + "quyufugai_" + "server_topic", rclcpp::QoS(10).best_effort(), std::bind(&quyufugai::service_status_callback, this, std::placeholders::_1));
    
    path_genearator.reset_params(top_left_x,top_left_y,width,height,vehicle_num,spacing,gap);
    this->pose.resize(this->vehicle_num);
    this->pose_sub.resize(this->vehicle_num);
    move_interface_pubs.resize(this->vehicle_num);
    move_interfaces.resize(this->vehicle_num);
    point_list_queue.resize(this->vehicle_num);
    arrived_first.resize(this->vehicle_num);
    first_received.resize(this->vehicle_num);
    for (size_t i = 0; i < vehicle_num; i++)
    {
        move_interfaces[i].is_body = false;
        move_interfaces[i].is_vel = true;
        move_interfaces[i].is_gps = false;
        first_received[i] = false;
        arrived_first[i] = false;
    }
    
    for (int i = 0; i < vehicle_num; ++i)
    {
        std::string topic_name = vehicle_type + "_" + std::to_string(i) + "/vision_pose/pose";
        pose_sub[i] = this->create_subscription<geometry_msgs::msg::PoseStamped>(topic_name, rclcpp::QoS(100).best_effort(),
            [this, i](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
                first_received[i] = true;
                this->pose[i] = *msg;
                RCLCPP_INFO(this->get_logger(), "Received Pose: x=%f, y=%f", msg->pose.position.x, msg->pose.position.y);
                
                geometry_msgs::msg::PoseStamped msg_temp;
                msg_temp.pose.position.x = point_list_queue[i].front().first;
                msg_temp.pose.position.y = point_list_queue[i].front().second;

                if (is_arrival(pose[i],msg_temp))
                {
                    // 无人机到达目标点后，需要计算速度
                    point_list_queue[i].pop();
                    geometry_msgs::msg::PoseStamped pose_msg_temp;
                    msg_temp.pose.position.x = point_list_queue[i].front().first;
                    msg_temp.pose.position.y = point_list_queue[i].front().second;
                    double vel_x = pose_msg_temp.pose.position.x - msg_temp.pose.position.x;
                    double vel_y = pose_msg_temp.pose.position.y - msg_temp.pose.position.y;
                    double magnitude = std::sqrt(vel_x * vel_x + vel_y * vel_y);
                    vel_x = vel_x / magnitude * speed;
                    vel_y = vel_y / magnitude * speed;
                    move_interfaces[i].vel.linear.x = vel_x;
                    move_interfaces[i].vel.linear.y = vel_y;
                    move_interfaces[i].vel.linear.z = 0;
                    arrived_first[i] = true;
                     RCLCPP_INFO(this->get_logger(), "Received Pose: x=%f, y=%f", msg->pose.position.x, msg->pose.position.y);

                }else{
                    if (arrived_first[i] == false)
                    {
                        double vel_x = msg_temp.pose.position.x - pose[i].pose.position.x;
                        double vel_y = msg_temp.pose.position.y - pose[i].pose.position.y;
                        double magnitude = std::sqrt(vel_x * vel_x + vel_y * vel_y);
                        vel_x = vel_x / magnitude * speed;
                        vel_y = vel_y / magnitude * speed;
                        move_interfaces[i].vel.linear.x = vel_x;
                        move_interfaces[i].vel.linear.y = vel_y;
                        move_interfaces[i].vel.linear.z = 0;
                    }
                    double vel_x = msg_temp.pose.position.x - pose[i].pose.position.x;
                    double vel_y = msg_temp.pose.position.y - pose[i].pose.position.y;
                    double magnitude = std::sqrt(vel_x * vel_x + vel_y * vel_y);
                    vel_x = vel_x / magnitude * speed;
                    vel_y = vel_y / magnitude * speed;
                    move_interfaces[i].vel.linear.x = vel_x;
                    move_interfaces[i].vel.linear.y = vel_y;
                    move_interfaces[i].vel.linear.z = 0;

                }
                
            });
        
    }

    for (int i = 0; i < vehicle_num; ++i)
    {
        std::string topic_names = "/uav_swarms/drone_"  + std::to_string(i) + "/move";
        move_interface_pubs[i] = this->create_publisher<yolo_detect_message::msg::MoveInterface>(topic_names, 10);
    }


    // 生成所有的路径，并且把路径的数据压入到堆栈中
    point_list = path_genearator.generatePaths();
    for (size_t i = 0; i < vehicle_num; i++)
    {
        if (point_list.empty())
        {
            RCLCPP_ERROR(this->get_logger(),"paths is empty");
        }
        
        for (auto & value : point_list[i])
        {
            point_list_queue[i].push(value);
        }
        
    }
    

    timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100), std::bind(&quyufugai::controlLoop, this));
    

}

void quyufugai::initialize_parameters() {
        // Declare parameters
    this->declare_parameter<std::string>("uav_namespace", "uav_swarms");
    this->declare_parameter<std::string>("vehicle_type", "drone");
    this->declare_parameter<int>("vehicle_num", 3);
    this->declare_parameter<double>("width", 10.0);
    this->declare_parameter<double>("height", 10.0);

    this->declare_parameter<double>("spacing", 2.0);
    this->declare_parameter<double>("gap", 1.0);

    this->declare_parameter<double>("left_x", 400.0);
    this->declare_parameter<double>("left_y", 40.0);

    // Get parameters
    vehicle_type = this->get_parameter("vehicle_type").as_string();
    namespaces = this->get_parameter("uav_namespace").as_string();
    vehicle_num = this->get_parameter("vehicle_num").as_int(); // Ensure vehicle_num is an integer
    width = this->get_parameter("width").as_double();
    height = this->get_parameter("height").as_double();
    spacing = this->get_parameter("spacing").as_double();
    gap = this->get_parameter("gap").as_double();
    top_left_x = this->get_parameter("left_x").as_double();
    top_left_y = this->get_parameter("left_y").as_double();

    // Log parameters
    RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());
    RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", vehicle_type.c_str());
    RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", vehicle_num);
    RCLCPP_INFO(this->get_logger(), "Width: %.2f", width);
    RCLCPP_INFO(this->get_logger(), "Height: %.2f", height);

    RCLCPP_INFO(this->get_logger(), "Spacing: %.2f", spacing);
    RCLCPP_INFO(this->get_logger(), "Gap: %.2f", gap);

    RCLCPP_INFO(this->get_logger(), "top_left_x: %.2f", top_left_x);
    RCLCPP_INFO(this->get_logger(), "top_left_y: %.2f", top_left_y);
}

void quyufugai::controlLoop(){

    if (service_status == false)
    {
        RCLCPP_INFO(this->get_logger(), "Server is starting...");
        return;
    }

    for (size_t i = 0; i < vehicle_num; i++)
    {
        if (first_received[i] == false)
        {
            return;
        }
        
        if (point_list_queue[i].empty())
        {
            move_interfaces[i].vel.linear.x = 0;
            move_interfaces[i].vel.linear.y = 0;
            move_interfaces[i].vel.linear.z = 0;
        }
        
        
        move_interface_pubs[i]->publish(move_interfaces[i]);
    }
    

}

void quyufugai::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
    bool status = msg->status;
    {
        std::lock_guard<std::mutex> lock(server_status_mutex_);
        service_status = status;
    }
    RCLCPP_INFO(this->get_logger(), "Service status changed to %d", service_status);
}


bool quyufugai::is_arrival(const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2, double threshold ) {
    // 计算位置的欧几里得距离
    double dx = pose1.pose.position.x - pose2.pose.position.x;
    double dy = pose1.pose.position.y - pose2.pose.position.y;
    // double dz = pose1.pose.position.z - pose2.pose.position.z;
    // double distance = std::sqrt(dx * dx + dy * dy + dz * dz);
     double distance = std::sqrt(dx * dx + dy * dy);
    // 判断距离是否小于阈值
    return distance < threshold;
}